#include  "servos.h"

/**
 * 电机初始化，本项目采用通用定时器3作为pwm输出
 */
void servos_init(void)
{
    GPIO_InitTypeDef GPIO_InitStructure={0};
    TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure={0};
    TIM_OCInitTypeDef   Tim_OCInitStructure={0};

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);

    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOA,&GPIO_InitStructure);

    TIM_TimeBaseStructure.TIM_Period=5000-1;     //50hz的舵机频率
    TIM_TimeBaseStructure.TIM_Prescaler=288-1;
    TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1;
    TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up;
    TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);

    Tim_OCInitStructure.TIM_Pulse=(uint16_t)(1500*0.25);        //375/5000=0.075
    Tim_OCInitStructure.TIM_OutputState=TIM_OutputState_Enable;
    Tim_OCInitStructure.TIM_OCMode=TIM_OCMode_PWM1;
    Tim_OCInitStructure.TIM_OCPolarity=TIM_OCPolarity_High;
    TIM_OC1Init(TIM3, &Tim_OCInitStructure);

    // TIM_CtrlPWMOutputs(TIM2, ENABLE);  只在tim1输出时有效
    TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Disable);
    TIM_ARRPreloadConfig(TIM3, ENABLE);
    TIM_Cmd(TIM3, ENABLE);
}

void set_servos_duty(int duty) {
    TIM_SetCompare1(TIM3, (uint16_t)(duty*0.25));
}

void set_servos_pwm(uint16_t pwm) {
    TIM_SetAutoreload(TIM3,pwm);
}
void set_servos_angle(int8_t angle) {
    set_servos_duty(SERVOS_MIDDLE_VAL+angle*1000/90);
}


